/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       ahrs_auxiliary.cpp
  * @author     baiyang
  * @date       2021-11-22
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include <stdbool.h>
#include <stdint.h>

#include "ahrs_ekf.h"
#include <EKF/ekf.h>

#include <uITC/uITC.h>
#include <uITC/uITC_msg.h>
#include <common/time/gp_time.h>

#include <sensor_imu/sensor_imu.h>
#include <sensor_baro/sensor_baro.h>
#include <sensor_compass/sensor_compass.h>
#include <sensor_gps/sensor_gps.h>
#include <common/geo/declination.h>
#include <ahrs/ahrs_view.h>
#include <parameter/param.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
void ahrs_ekf_try_set_origin(void);
/*----------------------------------variable----------------------------------*/
extern Ekf *_ekf;
extern parameters *_params;

uitc_vehicle_origin vehicle_origin;
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
/**
  * @brief       更新气压计数据
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
void ahrs_ekf_update_baro_sample()
{
    static uint64_t last_time_us = 0;
    uint64_t last_update_us = sensor_baro_get_last_update_usec();

    if (last_update_us != last_time_us) {
        float alt_m = sensor_baro_get_altitude_abs();
        _ekf->setBaroData(baroSample{(uint64_t)last_update_us, alt_m});

        last_time_us = last_update_us;
    }
}

void ahrs_ekf_update_compass_sample()
{
    static uint64_t last_time_us = 0;
    uint64_t last_update_us = sensor_compass_last_update_usec();

    if (last_update_us != last_time_us) {
        const Vector3f_t * mag = sensor_compass_get_field();
        magSample mag_sample {
            .time_us = last_update_us,
            .mag = Vector3f{
                mag->x*0.001f,
                mag->y*0.001f,
                mag->z*0.001f
            },
        };
        _ekf->setMagData(mag_sample);

        last_time_us = last_update_us;
    }
}

/**
  * @brief       
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
void ahrs_ekf_update_gps_sample()
{
    static uint32_t last_time_ms = 0;
    struct GPS_State* state = sensor_gps_primary_state();
    struct GPS_timing* timing = sensor_gps_primary_timing();

    if (timing->last_message_time_ms - last_time_ms > 70) {
        gps_message gps_msg{
                .time_usec = timing->last_message_time_ms*1000,
                .lat = state->location.lat,
                .lon = state->location.lng,
                .alt = state->location.alt,
                .yaw = 0,
                .yaw_offset = 0,
                .fix_type = state->status,
                .eph = state->horizontal_accuracy,
                .epv = state->vertical_accuracy,
                .sacc = state->speed_accuracy,
                .vel_m_s = state->ground_speed,
                .vel_ned = Vector3f{
                    state->velocity.x,
                    state->velocity.y,
                    state->velocity.z
                },
                .vel_ned_valid = state->have_speed_accuracy,
                .nsats = state->num_sats,
                .pdop = sqrtf(state->hdop *state->hdop
                          + state->vdop * state->vdop),
            };

        _ekf->setGpsData(gps_msg);

        last_time_ms = timing->last_message_time_ms;
    }
}

/**
  * @brief       
  * @param[in]   timestamp  
  * @param[out]  
  * @retval      
  * @note        
  */
void ahrs_ekf_publish_attitude(const uint64_t timestamp)
{
    if (_ekf->attitude_valid()) {
        // generate vehicle attitude quaternion data
        uitc_vehicle_attitude vehicle_attitude;
        const Quatf q{_ekf->calculate_quaternion()};

        vehicle_attitude.timestamp_us = timestamp;
        vehicle_attitude.vehicle_quat.q0 = q(0);
        vehicle_attitude.vehicle_quat.q1 = q(1);
        vehicle_attitude.vehicle_quat.q2 = q(2);
        vehicle_attitude.vehicle_quat.q3 = q(3);

        quat_to_euler(&vehicle_attitude.vehicle_quat, &vehicle_attitude.vehicle_euler);

        itc_publish(ITC_ID(vehicle_attitude),&vehicle_attitude);
    }
}

/**
  * @brief       
  * @param[in]   timestamp  
  * @param[out]  
  * @retval      
  * @note        
  */
void ahrs_ekf_publish_local_pos(const uint64_t timestamp)
{
    uitc_vehicle_alt vehicle_alt;
    uitc_vehicle_position vehicle_position;

    ahrs_ekf_try_set_origin();

    vehicle_alt.timestamp_us = timestamp;
    vehicle_position.timestamp_us = vehicle_alt.timestamp_us;

    // Position of body origin in local NED frame
    const Vector3f position = _ekf->getPosition();
    vehicle_position.x = position(0);
    vehicle_position.y = position(1);
    vehicle_alt.relative_origin = -position(2);
    vehicle_alt.relative_alt = -position(2) - get_ahrs_view()->arming_altitude_m;

    vehicle_alt.alt = -position(2) + vehicle_origin.alt;

    // Velocity of body origin in local NED frame (m/s)
    const Vector3f velocity = _ekf->getVelocity();
    vehicle_position.vx = velocity(0);
    vehicle_position.vy = velocity(1);
    vehicle_alt.vz = -velocity(2);

    // Acceleration of body origin in local frame
    Vector3f vel_deriv = _ekf->getVelocityDerivative();
    vehicle_position.ax = vel_deriv(0);
    vehicle_position.ay = vel_deriv(1);
    vehicle_alt.az = -vel_deriv(2);

    const Vector3f accel_bias{_ekf->getAccelBias()};

    vehicle_position.ax_bias = accel_bias(0);
    vehicle_position.ay_bias = accel_bias(1);
    vehicle_alt.az_bias = accel_bias(2);

    itc_publish(ITC_ID(vehicle_alt), &vehicle_alt);
    itc_publish(ITC_ID(vehicle_position), &vehicle_position);
}

/**
  * @brief       
  * @param[in]   
  * @param[out]  
  * @retval      
  * @note        
  */
void ahrs_ekf_try_set_origin(void)
{
    float ref_alt;
    uint64_t origin_time;
    double ekf_origin_lat;
    double ekf_origin_lon;
    estimator::ekf_solution_status soln_status;

    const bool ekf_origin_valid = _ekf->getEkfGlobalOrigin(origin_time, ekf_origin_lat, ekf_origin_lon, ref_alt);
    _ekf->get_ekf_soln_status((uint16_t*)&soln_status);

    if (!vehicle_origin.valid_alt && (soln_status.flags.pos_vert_abs || soln_status.flags.pos_vert_agl)) {
        vehicle_origin.timestamp_us = time_micros64();
        vehicle_origin.alt = sensor_baro_get_altitude_abs() + _ekf->getPosition()(2); //_ekf->getPosition()(2)朝下为正，所以这里是+
        vehicle_origin.z   = 0.0f;
        vehicle_origin.valid_alt = true;
        itc_publish(ITC_ID(vehicle_origin), &vehicle_origin);
    }

    if (ekf_origin_valid && !vehicle_origin.valid_hpos) {
        const Quatf q{_ekf->calculate_quaternion()};
        Quat_t vehicle_quat = {q(0), q(1), q(2), q(3)};
        Euler_t vehicle_euler;
        quat_to_euler(&vehicle_quat, &vehicle_euler);

        vehicle_origin.timestamp_us = time_micros64();
        vehicle_origin.lat = ekf_origin_lat * 1e7;
        vehicle_origin.lon = ekf_origin_lon * 1e7;

        vehicle_origin.x = _ekf->getPosition()(0);
        vehicle_origin.y = _ekf->getPosition()(1);

        vehicle_origin.yaw = vehicle_euler.yaw;
        vehicle_origin.mag_decl = geo_get_declination(ekf_origin_lat, ekf_origin_lon);
        vehicle_origin.valid_hpos = true;

        itc_publish(ITC_ID(vehicle_origin), &vehicle_origin);
    }

    if (!vehicle_origin.valid_lpos && vehicle_origin.valid_hpos && vehicle_origin.valid_alt) {
        vehicle_origin.timestamp_us = time_micros64();
        vehicle_origin.valid_lpos = true;
        itc_publish(ITC_ID(vehicle_origin), &vehicle_origin);
    }
}

/**
  * @brief       
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
void ahrs_ekf_assign_param()
{
    if (_params == nullptr) {
        return;
    }

    param_link_variable(PARAM_ID(EKF2, EKF2_GYR_NOISE), &_params->gyro_noise);
    param_link_variable(PARAM_ID(EKF2, EKF2_ACC_NOISE), &_params->accel_noise);
    param_link_variable(PARAM_ID(EKF2, EKF2_GYR_B_NOISE), &_params->gyro_bias_p_noise);
    param_link_variable(PARAM_ID(EKF2, EKF2_ACC_B_NOISE), &_params->accel_bias_p_noise);
    param_link_variable(PARAM_ID(EKF2, EKF2_MAG_E_NOISE), &_params->mage_p_noise);
    param_link_variable(PARAM_ID(EKF2, EKF2_MAG_B_NOISE), &_params->magb_p_noise);
    param_link_variable(PARAM_ID(EKF2, EKF2_WIND_NOISE), &_params->wind_vel_p_noise);
    param_link_variable(PARAM_ID(EKF2, EKF2_GND_EFF_DZ), &_params->gnd_effect_deadzone);
    param_link_variable(PARAM_ID(EKF2, EKF2_GND_MAX_HGT), &_params->gnd_effect_max_hgt);
    param_link_variable(PARAM_ID(EKF2, EKF2_HEAD_NOISE), &_params->mag_heading_noise);
    param_link_variable(PARAM_ID(EKF2, EKF2_MAG_NOISE), &_params->mag_noise);
    param_link_variable(PARAM_ID(EKF2, EKF2_EAS_NOISE), &_params->eas_noise);
    param_link_variable(PARAM_ID(EKF2, EKF2_BETA_NOISE), &_params->beta_noise);
    param_link_variable(PARAM_ID(EKF2, EKF2_NOAID_NOISE), &_params->pos_noaid_noise);
    param_link_variable(PARAM_ID(EKF2, EKF2_BARO_NOISE), &_params->baro_noise);
    param_link_variable(PARAM_ID(EKF2, EKF2_MAG_DECL), &_params->mag_declination_deg);
    param_link_variable(PARAM_ID(EKF2, EKF2_DECL_TYPE), &_params->mag_declination_source);
    param_link_variable(PARAM_ID(EKF2, EKF2_MAG_TYPE), &_params->mag_fusion_type);
    param_link_variable(PARAM_ID(EKF2, EKF2_GPS_CHECK), &_params->gps_check_mask);
    param_link_variable(PARAM_ID(EKF2, EKF2_AID_MASK), &_params->fusion_mode);
    param_link_variable(PARAM_ID(EKF2, EKF2_HGT_MODE), &_params->vdist_sensor_type);
    param_link_variable(PARAM_ID(EKF2, EKF2_TERR_MASK), &_params->terrain_fusion_mode);
    param_link_variable(PARAM_ID(EKF2, EKF2_RNG_NOISE), &_params->range_noise);
    param_link_variable(PARAM_ID(EKF2, EKF2_RNG_AID), &_params->range_aid);
    param_link_variable(PARAM_ID(EKF2, EKF2_EVV_GATE), &_params->ev_vel_innov_gate);
    param_link_variable(PARAM_ID(EKF2, EKF2_EVP_GATE), &_params->ev_pos_innov_gate);
    param_link_variable(PARAM_ID(EKF2, EKF2_OF_N_MIN), &_params->flow_noise);
    param_link_variable(PARAM_ID(EKF2, EKF2_OF_N_MAX), &_params->flow_noise_qual_min);
    param_link_variable(PARAM_ID(EKF2, EKF2_ARSP_THR), &_params->arsp_thr);
    param_link_variable(PARAM_ID(EKF2, EKF2_DRAG_NOISE), &_params->drag_noise);
    param_link_variable(PARAM_ID(EKF2, EKF2_MAG_CHECK), &_params->check_mag_strength);
    param_link_variable(PARAM_ID(EKF2, EKF2_SYNT_MAG_Z), &_params->synthesize_mag_z);
}

/*------------------------------------test------------------------------------*/


